/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "dma.h"
#include "app_fatfs.h"
#include "spi.h"
#include "usart.h"
#include "usb_device.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "mpu6050.h"
#include "oled.h"
#include "gnss.h"		 
#include "ds18b20.h"
#include "hdsp2502.h"
#include "filter.h"
#include "attribute.h"
#include "W25QXX.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
#define uart_rx_buffer_size 2048
uint8_t uart_rx_buffer[uart_rx_buffer_size];
uint32_t revlen = 0;
extern DMA_HandleTypeDef hdma_usart1_rx;
extern FRESULT  fileSystemInit(void);
uint8_t gnss_data_ready=0;
nmea_msg gps_info;
float tem,bat_val;
accData_t accData;
gyroData_t gyroData;
lpfData_t accLpfData[3];
lpfData_t gyroLpfData[3];
attribute_t attribute; 
float pitch_offset=0;
float roll_offset=0;
float yaw_offset=0;
float slope;
float slope_imu;
float slope_gps;
float avg_speed;
float total_km=0;
float lat_last,lon_last;
float slope_dis_last=0;
float slope_alt_last=0;
float slope_angle_gps=0;
unsigned int PPS_CNT;
char gps_cnt=0;
char pause_flag=1;
FATFS fs;
FIL file;
FIL *pf = &file; 
UINT bw;
float theta;
float lon_last_slope=0;
float acc=0;
int acc_dr_cnt=0;
float coef_acc=4.5f;
float combine_speed=0.0f;
extern char gps_vel_invalid;
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_SPI2_Init();
  MX_USART1_UART_Init();
  if (MX_FATFS_Init() != APP_OK) {
    Error_Handler();
  }
  MX_USB_Device_Init();
  MX_ADC1_Init();
  /* USER CODE BEGIN 2 */
	HAL_Delay(100);
	Init_MPU6050();
	OLED_Init();
	//fileSystemInit();
	uint16_t cnt6050=0;
	HAL_UART_Receive_DMA(&huart1,uart_rx_buffer,sizeof(uart_rx_buffer));
	char disp_buffer[64];
	imuLpfFilterInit(&accLpfData[0],&gyroLpfData[0]);
	OLED_ShowString(0,0,(uint8_t *)"Init...",16,1);
	OLED_Refresh();
	while(cnt6050<500)
	{
		cnt6050++;
		Refresh6050();
	}
//	pitch_offset=attribute.pitch;
//	roll_offset=attribute.roll;
//	yaw_offset=attribute.yaw;
	theta=asin(accData.accY/1);
	OLED_ShowString(0,0,(uint8_t *)"        ",16,1);
	OLED_Refresh();
//	f_open(pf, "test.txt", FA_OPEN_ALWAYS | FA_WRITE);
//	f_write(&file, "HHHHH", sizeof("HHHHH"), &bw);
//	f_close(pf);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		if(gnss_data_ready==1)
		{
			if(acc_dr_cnt>5000)
			{
				acc_dr_cnt=0;
				OLED_Init();
			}
			gnss_data_ready=0;
			NMEA_Analysis(&gps_info,uart_rx_buffer);

//			sprintf(disp_buffer,"LON:%f",(float)gps_info.longitude/100000.0f);
//			OLED_ShowString(0,0,(uint8_t *)disp_buffer,8,1);
//			sprintf(disp_buffer,"LAT:%f",(float)gps_info.latitude/100000.0f);
//			OLED_ShowString(0,8,(uint8_t *)disp_buffer,8,1);
//			sprintf(disp_buffer,"VEL:%fkph",(float)gps_info.speed/1000.0f);
//			OLED_ShowString(0,16,(uint8_t *)disp_buffer,8,1);

			if(gps_info.speed>1500)
				pause_flag=0;
			else
				pause_flag=1;
			sprintf(disp_buffer,"ALT");
			OLED_ShowString(64,56,(uint8_t *)disp_buffer,8,0);
			sprintf(disp_buffer,"%.1fm",(float)gps_info.altitude/1000.0f);
			OLED_ShowString(84,56,(uint8_t *)disp_buffer,8,1);
			if(gps_info.fixmode==3||gps_info.fixmode==2)
			{
				gps_cnt++;
				if((gps_info.latitude!=0)&&(lat_last!=0))
				{ 
					if((gps_info.latitude-lat_last>10000)||(gps_info.longitude-lon_last>10000))
					{
						gps_info.latitude=lat_last;
						gps_info.longitude=lon_last;
					}
					total_km+=0.99f*get_distance(gps_info.latitude/100000.0f,gps_info.longitude/100000.0f,lat_last/100000.0f,lon_last/100000.0f);
				}
				if(total_km-slope_dis_last>0.03f)
				{
					if(pause_flag==0)
					{
						slope_gps=(((float)gps_info.altitude/1000.0f)-slope_alt_last)/((total_km-slope_dis_last)*1000);
						slope_angle_gps=atan(slope_gps)*180.0f/PI;
						roll_offset=attribute.roll-slope_angle_gps;
						slope_dis_last=total_km;
						slope_alt_last=(float)gps_info.altitude/1000.0f;
					}
				}
				else
				{
					
				}
				sprintf(disp_buffer,"%04d-%02d-%02d  %02d:%02d:%02d",gps_info.utc.year,gps_info.utc.month,gps_info.utc.date,gps_info.utc.hour,gps_info.utc.min,gps_info.utc.sec);
			}
			else
			{
				sprintf(disp_buffer,"GNSS Searching...            ");
			}
			OLED_ShowString(0,0,(uint8_t *)disp_buffer,8,1);
			if(gps_info.fixmode==3||gps_info.fixmode==2)
			{
				lon_last=gps_info.longitude;
				lat_last=gps_info.latitude;
				sprintf(disp_buffer,"%02d:%02d:%02d",PPS_CNT/3600,PPS_CNT%3600/60,PPS_CNT%60);
			}
			else
			{
				sprintf(disp_buffer,"--:--:--");
			}
			OLED_ShowString(70,12,(uint8_t *)disp_buffer,8,1);
			sprintf(disp_buffer,"FIX");
			OLED_ShowString(64,48,(uint8_t *)disp_buffer,8,0);
			if(gps_info.fixmode==2)
				sprintf(disp_buffer,"2D");
			else if(gps_info.fixmode==3)
				sprintf(disp_buffer,"3D");
			else
				sprintf(disp_buffer,"X ");
			OLED_ShowString(84,48,(uint8_t *)disp_buffer,8,1);
			OLED_DrawLine(0,10,128,10,1);
			sprintf(disp_buffer,"AVG");
			OLED_ShowString(64,32,(uint8_t *)disp_buffer,8,0);
			if(PPS_CNT>10)
				avg_speed=total_km/((float)PPS_CNT/3600.0f);
			sprintf(disp_buffer,"%.1fKPH  ",avg_speed);
			OLED_ShowString(84,32,(uint8_t *)disp_buffer,8,1);
			sprintf(disp_buffer,"VEL");
			OLED_ShowString(64,24,(uint8_t *)disp_buffer,8,0);
			sprintf(disp_buffer,"%5.1fKPH",(float)gps_info.speed/1000.0f);
			hdlp_disp_str(disp_buffer);
			sprintf(disp_buffer,"%.1fKPH  ",(float)gps_info.speed/1000.0f);
			combine_speed=(float)gps_info.speed/1000.0f;
			OLED_ShowString(84,24,(uint8_t *)disp_buffer,8,1);
			sprintf(disp_buffer,"%03.2fKM  ",total_km);
			OLED_ShowString(0,18,(uint8_t *)disp_buffer,16,1);
			memset(uart_rx_buffer,0,uart_rx_buffer_size);
			HAL_UART_Receive_DMA(&huart1,uart_rx_buffer,sizeof(uart_rx_buffer));
			HAL_GPIO_TogglePin(LED_REC_GPIO_Port,LED_REC_Pin);
		}
		else
		{
			acc=9.801f*((accData.accZ*sin(theta))-(accData.accY*cos(theta)));
//			acc=9.801f*accData.accY;
//			acc_dr+=acc;
//			acc_dr_cnt++;
//			if((acc_dr_cnt%10)==0)
//			{
//				combine_speed+=(coef_acc*acc_dr);
//				acc_dr=0;
//				if(combine_speed<0.0f)
//					combine_speed=0.0f;
//				sprintf(disp_buffer,"%5.1fKPH",combine_speed/1000.0f);
//				hdlp_disp_str(disp_buffer);
//			}
			if(acc>0.0f)
				sprintf(disp_buffer,"ACC %3.02f  ",acc);
			else
				sprintf(disp_buffer,"ACC%3.02f  ",acc);
			OLED_ShowString(0,12,(uint8_t *)disp_buffer,8,1);
//			sprintf(disp_buffer,"GX%3.1f GY%.1f GZ%3.1f",gyroData.gyroX_deg,gyroData.gyroY_deg,gyroData.gyroZ_deg);
		//	OLED_ShowString(0,8,(uint8_t *)disp_buffer,8,1);
//			sprintf(disp_buffer,"P%.1f R%.1f Y%.1f",attribute.pitch-pitch_offset,attribute.roll-roll_offset,attribute.yaw-yaw_offset);
//			OLED_ShowString(0,16,(uint8_t *)disp_buffer,8,1);
			
			if(slope_gps>0)
			{
				if(slope_gps>0.99f)
					slope_gps=0.99f;
				sprintf(disp_buffer," %.1f",slope_gps*100.0f);
				OLED_ShowString(0,33,(uint8_t *)disp_buffer,16,1);
			}
			else
			{
				if(slope_gps<-0.99f)
					slope_gps=-0.99f;
				sprintf(disp_buffer,"%.1f",slope_gps*100.0f);
				OLED_ShowString(0,33,(uint8_t *)disp_buffer,16,1);
			}
			OLED_ShowString(42,33,(uint8_t *)"%",16,1);
			tem=(float)DS18B20_Get_Temp()/10;
			sprintf(disp_buffer,"%.1f",tem);
			OLED_ShowString(0,48,(uint8_t *)disp_buffer,16,1);
			OLED_ShowChinese(36,48,4,16,1);
			bat_val=bat_get();
			sprintf(disp_buffer,"BAT");
			OLED_ShowString(64,40,(uint8_t *)disp_buffer,8,0);
			sprintf(disp_buffer,"%.1fV",bat_val);
			OLED_ShowString(84,40,(uint8_t *)disp_buffer,8,1);
			//OLED_DrawLine(63,64,63,38,1);
			OLED_DrawLine(64,20,128,20,1);
			OLED_DrawLine(64,21,83,21,1);
			OLED_DrawLine(64,22,83,22,1);
			OLED_DrawLine(64,23,83,23,1);
			OLED_DrawLine(83,20,83,64,1);
			OLED_DrawLine(82,20,82,64,1);
			Refresh6050();
		}
		OLED_Refresh();
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48|RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSI48State = RCC_HSI48_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV1;
  RCC_OscInitStruct.PLL.PLLN = 28;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV6;
  RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(GPIO_Pin == GPIO_PIN_8)				
	{
		if(pause_flag==0)
			PPS_CNT++;
	}
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
